#include <mc_common/math_utils.h>

namespace math_utils{

/************************************
 * 角度计算
 * https://wiki.ros.org/angles
 *************************************/

// 弧度转角度
float Rad2Deg(float radians){
    return radians * 180.0 / M_PI;
}

// 角度转弧度
float Deg2Rad(float degrees){
    return degrees * M_PI / 180.0;
}

// 角度转换 [-PI,PI]
float NorMalizeAngle(float angle){
  const float result = fmod(angle + M_PI, 2.0 * M_PI);
  if (result <= 0.0)
    return result + M_PI;
  return result - M_PI;
}

// 角度差 b - a
float DifferenceAngle(float a, float b){
    return NorMalizeAngle(b - a);
}

// 角度相加
float AdditionAngle(float a, float b){
    return NorMalizeAngle(a + b);
}


// 计算两个点之间的角度
float AnglePose2D(Pose2D a, Pose2D b){
  float dx = b[0] - a[0];   // x1 - x2
  float dy = b[1] - a[1];   // y1 - y2
  return atan2(dy, dx);
}

// 计算两点之间距离
float DistancePose2D(Pose2D a, Pose2D b){
  float dx = b[0] - a[0];   // x1 - x2
  float dy = b[1] - a[1];   // y1 - y2
  return hypot(dx, dy);
}

// 位姿转换
Pose2D Pose2DGlobal2Local(Pose2D curr, Pose2D global_pose){
    Pose2D local_pose;
    float delta_x = global_pose[0] - curr[0];
    float delta_y = global_pose[1] - curr[1];
    float cos_yaw = cos(-curr[2]);
    float sin_yaw = sin(-curr[2]);
    local_pose[0] = delta_x * cos_yaw - delta_y * sin_yaw;
    local_pose[1] = delta_x * sin_yaw + delta_y * cos_yaw;
    local_pose[2] = DifferenceAngle(curr[2], global_pose[2]);
    return local_pose;
}

// 位姿转换
Pose2D Pose2DLocal2Global(Pose2D curr, Pose2D local_pose){
    Pose2D global_pose;
    float cos_yaw = std::cos(curr[2]);
    float sin_yaw = std::sin(curr[2]);
    global_pose[0] = curr[0] + local_pose[0] * cos_yaw - local_pose[1] * sin_yaw;
    global_pose[1] = curr[1] + local_pose[0] * sin_yaw + local_pose[1] * cos_yaw;
    global_pose[2] = AdditionAngle(curr[2], local_pose[2]);
    return global_pose;
}

// Path 路径转换
vector<Pose2D> PathGlobal2Local(Pose2D curr, vector<Pose2D>& points){
    vector<Pose2D> local_points;
    float cos_yaw = cos(-curr[2]);
    float sin_yaw = sin(-curr[2]);
    for(auto global_pose : points){
        Pose2D local_pose;
        float delta_x = global_pose[0] - curr[0];
        float delta_y = global_pose[1] - curr[1];
        local_pose[0] = delta_x * cos_yaw - delta_y * sin_yaw;
        local_pose[1] = delta_x * sin_yaw + delta_y * cos_yaw;
        local_pose[2] = DifferenceAngle(curr[2], global_pose[2]);
        local_points.emplace_back(local_pose);
    }
    return local_points;
}



}; //math_utils 